/* ============================================================
 * Part of 'Autostop' project
 * Under GPL license - http://www.gnu.org/licenses/gpl.html
 * (c) 2006 - Autostop team
 */

///\file camera.c implementation of the camera driver-to-userspace library

#include <fcntl.h>
#include <sys/stat.h>
#include <stdlib.h>
#include <sys/time.h>
#include <stdio.h>

#include "registers.h"
#include "camera.h"

// =================
// consts

/// default device file name
#define TOOLS_DEFAULT_DEVICE	"/autostop/dev/camera"


// bit mask -readed images:
// FIXME this disables raw imagwes permanentely!
#define READED_IMAGES	( CCR_ENRX | CCR_ENJ | CCR_ENYX )
//#define READED_IMAGES	( CCR_ENRX )
// TODO add CCR_ENYX ?
// TODO jpeg disabled while it spols movemetn data
//#define READED_IMAGES	CCR_ENRX

// =================
// globals

const char* error_message	=	"";		// error message
int device = -1;						// device handler

int jpeg_size	= 8;					// jpeg frame size. TODO: Should be read form aux processor!


// ========================================================
int camera_init(void)
{
	// try to open device
	device = open( TOOLS_DEFAULT_DEVICE, O_RDONLY );
	if ( device < 0 ) {
		error_message = "could not open device file " TOOLS_DEFAULT_DEVICE;
		return CAMERA_ERROR;
	}
	
	// set camera in raw reading mode
	//write_register( device, CWR_CR, CCR_ENYX ); commented - now set in getimage methods
	
	return CAMERA_OK;
}

// ========================================================
int camera_image_width(void)
{
	return 640;
}

// ========================================================
int camera_image_height(void)
{
	return 480;
}

// ========================================================
int camera_get_image( uint8_t* pbuffer, size_t bufsize, uint64_t* ptimestamp )
{
	int waiting = 0;	// waiting [us]
	struct timeval start, end;
	uint64_t startms, endms;
	
	// check device
	if ( device < 0 ) {
		error_message = "not initilaized";
		return CAMERA_ERROR;
	}
	
	// set 'enable' bit
	write_register( device, CWR_CR, CCR_ENYX );
	
	// wait for 'ready' bit
	while( ! ( read_register( device, CRD_STAT1 ) & ( CSTAT1_JNBY | CSTAT1_JNBY2 ) ) )
	{
		// wait a bit
		usleep( 10 );
		// inc waiting counter
		waiting += 10;
		// check if this is sane time
		if (waiting > 10000 )
		{
			error_message = "timeout waiting for image data";
			return CAMERA_ERROR;
		}
	}
	
	// TODO debug
	//printf("witing for image: %d us\n", waiting );
	
	// get start time
	gettimeofday( & start, NULL );
	startms = (uint64_t)start.tv_sec * 1000 + start.tv_usec / 1000;
	
	// copy time to user supplied buffer
	if ( ptimestamp )
	{
		*ptimestamp = startms;
	}
	
	// set 'read' bit
	write_register( device, CWR_CR, READED_IMAGES | CCR_CZYTYX );
	
	// read header
	read_register( device, CRD_FF ); // ignore for now
	read_register( device, CRD_FF );
	
	// read
	// cool new modern and hyperfast
	read_multi_hibytes( device, CRD_FF, pbuffer, bufsize );
	
	
	// clear 'read' bit
	write_register( device, CWR_CR, CCR_ENYX );
	
	// get end time (DEBUG)
	gettimeofday( & end, NULL );
	endms = (uint64_t)end.tv_sec * 1000 + end.tv_usec / 1000;
	
	// display total time (DEBUG)
	//printf("reading frame: %d ms\n", (int)(endms-startms) );
	
	return CAMERA_OK;
}

// ========================================================
uint64_t camera_get_time()
{
	struct timeval t;
	gettimeofday( & t, NULL );
	return (uint64_t)t.tv_sec * 1000 + t.tv_usec / 1000;
}

// ========================================================
sequence_t camera_open_sequence(void)
{
	return SEQUENCE_INVALID;
}

// ========================================================
int camera_read_sequence( sequence_t sequence, uint8_t* pbuffer, size_t bufsize )
{
	return CAMERA_ERROR;
}

// ========================================================
void camera_close_sequence( sequence_t sequence )
{
}

// ========================================================
const char* camera_error_message()
{
	return error_message;
}

// ========================================================
int camera_get_segmented_image( uint8_t* pbuffer, size_t bufsize, uint64_t* ptimestamp )
{
	const int total_bytes_to_ignore = 4;
	const int bytes_in_fifo = 128; // bytes waiting in fifo when FFNZ bit is active
	
	int waiting = 0;	// waiting [us]
	int i = 0 ; // index
	int bytes_to_ignore = total_bytes_to_ignore;
	int bytes_to_read = bufsize + bytes_to_ignore;
	struct timeval start, end;
	uint64_t startms, endms;
	uint8_t ignored_bytes[ bytes_to_ignore ];
	
	// check device
	if ( device < 0 ) {
		error_message = "not initilaized";
		return CAMERA_ERROR;
	}
	
	if ( ptimestamp )
	{
		// TODO use fpga time, but make cmera_get_time also use it
		//*ptimestamp = ( ignored_bytes[ 1 ] << 8 ) + ignored_bytes[ 0 ];
		*ptimestamp = camera_get_time();
	}
	
	// set 'enable' bit
	write_register( device, CWR_CR, READED_IMAGES );
	
	// wait for 'ready' bit
	while( ! (  ( i = read_register( device, CRD_STAT1 ) ) & CSTAT1_JNBYY ) )
	{
		// wait a bit
		usleep( 100 );
		// inc waiting counter
		waiting += 10;
		// check if this is sane time
		if (waiting > 10000 )
		{
			error_message = "timeout waiting for segmented image data";
			return CAMERA_ERROR;
		}
	}
	/**/
	
	// set 'read' bit
	write_register( device, CWR_CR, READED_IMAGES | CCR_CZYTRX );
	
	
	// now, this is proper implementation:
	while( bytes_to_read > 0 )
	{
		int read_this_time = bytes_to_read > bytes_in_fifo ? bytes_in_fifo : bytes_to_read; // how many bytes read this time
		int waiting = 0;	// usecond counter
		
		// wait for ready bit
		while( ! ( read_register( device, CRD_STAT ) & CSTAT_FFNZ ) )
		{
			// wait a bit
			usleep( 10 );
			// inc waiting counter
			waiting += 10;
			// check if this is sane time
			if (waiting > 10000 )
			{
				error_message = "timeout waiting for fifo";
				return CAMERA_ERROR;
			}
		}
		
		// read in pairs
		for( i = 0; i < read_this_time; i+=2 )
		{
			int word;
			uint8_t hibyte, lobyte;
			
			word = read_register( device, CRD_FF );
			
			lobyte = word & 0xff;
			hibyte = ( word & 0xff00 ) >> 8;
			
			// ignore?
			if ( bytes_to_ignore > 0 )
			{
				ignored_bytes[ total_bytes_to_ignore - bytes_to_ignore   ] = lobyte;
				ignored_bytes[ total_bytes_to_ignore - bytes_to_ignore+1 ] = hibyte;
				bytes_to_ignore -= 2;
			}
			// ok, add to buffer
			else
			{
				*(pbuffer++) = lobyte;
				*(pbuffer++) = hibyte;
			}
		}
		
		// decrement byte counter
		bytes_to_read -= read_this_time;
	}
	
	// clear 'read' bit
	write_register( device, CWR_CR, READED_IMAGES );
		
	// copy time to user supplied buffer
	if ( ptimestamp )
	{
		// TODO use fpga time, but make cmera_get_time also use it
		//*ptimestamp = ( ignored_bytes[ 1 ] << 8 ) + ignored_bytes[ 0 ];
	}
	

	return CAMERA_OK;
}

// ========================================================
int camera_get_jpeg( uint8_t* pbuffer, size_t bufsize, size_t* imagesize, uint64_t* timestamp )
{
	int waiting = 0;	// waiting [us]
	int i = 0 ; // index
	size_t jpegsize = 0;	// size of jpeg image
	struct timeval start, end;
	uint64_t startms, endms;
	const int JPG_HEADER_SIZE = 16; // there is a 16-bytes header grued into fro of jpeg2k data
	
	// get timestamp
	if ( timestamp )
	{
		*timestamp = camera_get_time(); // TODO read from frame
	}
	
	// check device
	if ( device < 0 ) {
		error_message = "not initilaized";
		return CAMERA_ERROR;
	}
	
	// set 'enable' bit
	write_register( device, CWR_CR, READED_IMAGES );
	
	// wait for 'ready' bit
	while( ! ( ( i = read_register( device, CRD_STAT1 ) ) & CSTAT1_JNBJ  ) )
	{
		//printf("waiting for jpeg: 0x%x\n", i); // TODO remove i and printf
		// wait a bit
		usleep( 10 );
		// inc waiting counter
		waiting += 10;
		// check if this is sane time
		if (waiting > 10000 )
		{
			error_message = "timeout waiting for jpeg image data";
			return CAMERA_ERROR;
		}
	}
	
	// get rid of queued images
	while( read_register( device, CRD_STAT1 ) & CSTAT1_JNBJ2 )
	{
		// set rerad bit up'n'down
		write_register( device, CWR_CR, READED_IMAGES | CCR_CZYTJX );
		write_register( device, CWR_CR, READED_IMAGES );
	}
	
	// set 'read' bit
	write_register( device, CWR_CR, READED_IMAGES | CCR_CZYTJX );
	
	// wait for 'fifo-no-zero' bit
	while( ! ( ( i = read_register( device, CRD_STAT ) ) & CSTAT_FFNZ ) )
	{
		//printf("jpeg: waiting for fifo 0x%x\n", i ); // TODO remove i and printf
		// wait a bit
		usleep( 10 );
		// inc waiting counter
		waiting += 10;
		// check if this is sane time
		if (waiting > 10000 )
		{
			error_message = "timeout waiting for fifo";
			return CAMERA_ERROR;
		}
	}
	// get jpeg size [in bytes]
	jpegsize = read_register( device,  CRD_ILEJ ) << 2; // shifted by two, as CRD_ILEJ contains size in dwords
	jpegsize -= JPG_HEADER_SIZE;	// we do not count the header
	
	// write out
	if ( imagesize )
	{
		*imagesize = jpegsize;
	}
	
	// check buffer size
	if ( jpegsize > bufsize )
	{
		error_message = "jpeg buffer too small";
		return CAMERA_SMALLBUFFER;
	}
	
	// read our header (timestamp and number)
	read_register( device, CRD_FF ); // ignore for now
	read_register( device, CRD_FF );
	
	// read header generated by jpg2k chipo
	for ( i = 0; i < (JPG_HEADER_SIZE >> 1); i++)
	{
		read_register( device, CRD_FF );
	}
	
	// read data
	read_multi_bytes( device, CRD_FF, pbuffer, jpegsize );
	
	// clear 'read' bit
	write_register( device, CWR_CR, READED_IMAGES );
	
	return CAMERA_OK;
}

// ========================================================
int camera_set_bgfactor( uint16_t bgfactor )
{
	// check device
	if ( device < 0 ) {
		error_message = "not initilaized";
		return CAMERA_ERROR;
	}
	
	write_register( device, CWR_BGFACTOR, bgfactor );

	return CAMERA_OK;
}

// ========================================================
int camera_set_fgtreshold( uint8_t fgtreshold  )
{
	// check device
	if ( device < 0 ) {
		error_message = "not initilaized";
		return CAMERA_ERROR;
	}
	write_register( device, CWR_FGTRESHOLD, fgtreshold );

	return CAMERA_OK;
}

// ========================================================
/// Sends text command via command pipe to auxiliary processor.
///\par cmd	- command for auxiliary processor. Should be terminated with '\r'
///\return status code
static int _camera_send_command( const char* cmd )
{
	const int	fifo_size	= 64; 		// data may be written in this sized chunks. otherwise - fullxp byte has to be checked
	const int	tiemout 	= 10000;	// wait timeout [us]
	const int 	wait_bit 	= 10;		// single wait period [us]
	int 		i;						// index
	
	// check device
	if ( device < 0 ) {
		error_message = "not initilaized";
		return CAMERA_ERROR;
	}
	
	// repeat until written
	while( *cmd )
	{
		
		int	waiting = 0;	 	// waiting time [us]
		
		// wait for fullxp bit down
		while( ( read_register( device, CRD_STAT ) & CSTAT_FULLXP ) )
		{
			// wait a bit
			usleep( wait_bit );
			// inc waiting counter
			waiting += wait_bit;
			// check if this is sane time
			if (waiting > tiemout )
			{
				error_message = "timeout waiting for command fifo";
				return CAMERA_ERROR;
			}
		}
		
		// ok, may send
		for( i = 0; ( i < fifo_size ) && *cmd ; i++, cmd++ )
		{
			write_register( device, CWR_FF, *cmd );
			usleep( 500 );	// TEST: some wait to make sure the character reach it's place
		}
	}
	
	// happy end
	return CAMERA_OK;
}

// ========================================================
/// Sets CCD register. 
///\warning this is CCD-dependand command and will be removed in future
///\par page	- register page number
///\par addr	- register address
///\par value	- desired register value
///\return status code
static int _camera_write_ccd_register( int page, int addr, int value )
{
	const int size = 64;
	char page_command[ size ];
	char reg_command[ size ];
	
	// prepare strings
	snprintf( page_command, size, "wrccd 240 %d\r", page );
	snprintf( reg_command, size, "wrccd %d %d\r", addr, value );
	
	// go
	if ( _camera_send_command( page_command ) == CAMERA_OK )
	{
		return _camera_send_command( reg_command );
	}
	
	// error string set by higher level (oh, i miss c++ exceptions... )
	return CAMERA_ERROR;
}

// ========================================================
int camera_jpeg_set_shrink( int shrink )
{
	return _camera_send_command( shrink ? "dzxy 1\r" : "dzxy 0\r" );
}

// ========================================================
int camera_jpeg_set_quality( int size )
{
	char cmd[ 64];
	
	jpeg_size = size;
	
	snprintf( cmd, 64, "size %d\r", size );
	return _camera_send_command( cmd );
}

// ========================================================
int camera_get_jpeg_quality()
{
	return jpeg_size;
}

// ========================================================
int camera_set_exposure( int exposure )
{
	const int multiplier = 18; // in this particulat CCD exposure is expressed in 1/18 of ms
	return _camera_write_ccd_register( 0, 9, exposure * multiplier ); 
}

// ========================================================
int camera_set_auto_exposure( int auto_exposure )
{
	return _camera_write_ccd_register( 1, 6, auto_exposure ? 0x700e : 0x300c ); 
}

// ========================================================
double camera_accel_get_x()
{
	int reference = read_register( device, CRD_ACCEL_XR );
	if ( reference > 0 )
	{
	
		return (double)read_register( device, CRD_ACCEL_XV ) / (double)reference;
	}
	
	return 0.0;
}

// ========================================================
double camera_accel_get_y()
{
	int reference = read_register( device, CRD_ACCEL_YR );
	if ( reference > 0 )
	{
	
		return (double)read_register( device, CRD_ACCEL_YV ) / (double)reference;
	}
	
	return 0.0;
}

// eof

